/*
 * @Author: your name
 * @Date: 2021-05-01 19:36:26
 * @LastEditTime: 2021-05-06 19:03:10
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \gcxz\Middlewares\Device\chassis.cpp
 */

#include "chassis.h"

void Chassis::control(){

    controlPose();  // 得到x,y,z目标速度

    resolve();      // 由x,y,z的速度解算出底盘电机的pwm值
    
    for(int i = 0; i < 4; i++)
        motors[i].control();
}

void Chassis::init(){
    for(int i = 0; i < 4; i++)
        motors[i].init();
}

void Chassis::setMotorPid(float _Kp, float _Ki, float _Kd, 
	float _I_Term_Max, float _Out_Max){
    
    for (int i = 0; i < 4; i++)
		motors[i].pid.SetPIDParam(_Kp, _Ki, _Kd, _I_Term_Max, _Out_Max);
}

void Chassis::setTargetPose(Vector3 pose){
    target_pose = pose;
}

void Chassis::updateCurrentPose(Vector3 pose){
    current_pose = pose;
}
// 巡航用
void Chassis::controlPose(){
	
	pose_pid[X].Target = target_pose.x;
    pose_pid[Y].Target = target_pose.y;
    pose_pid[Z].Target = target_pose.z;
	
	pose_pid[X].Current = current_pose.x;
    pose_pid[Y].Current = current_pose.y;
    pose_pid[Z].Current = current_pose.z;
	
    Vector3 vel;
    vel.x = pose_pid[X].openloop();    // 输入等于输出，加入了限幅
    vel.y = pose_pid[Y].Adjust();
    vel.z = pose_pid[Z].Adjust();    // 比例控制

    setTargetVel(vel);               // 将pid出来的速度传到目标速度那里，进行速度限幅
}

// 用于地标前的调整
 void Chassis::adjustPose(){
    pose_pid[X].Target = 0;
    pose_pid[Y].Target = 0;
    pose_pid[Z].Target = 0;
	
	pose_pid[X].Current = current_pose.x;
    pose_pid[Y].Current = current_pose.y;
    pose_pid[Z].Current = current_pose.z;
	
    Vector3 vel;
    vel.x = pose_pid[X].Adjust();    // 输入等于输出，加入了限幅
    vel.y = pose_pid[Y].Adjust();
    vel.z = pose_pid[Z].Adjust();    // 比例控制
    if((current_pose.y > 7)|| (current_pose.y < -7))
    {   
        if(vel.y >=0){
            vel.y = 2000 + vel.y;
        }
        else{
            vel.y = -2000 + vel.y;
        }
    }
    else if ((current_pose.x > 7)|| (current_pose.x < -7))
    {
        if(vel.x >=0){
            vel.x = 1800 + vel.x;
        }
        else{
            vel.x = -1800 +vel.x;
        }
    } 
    
    setTargetVel(vel);               // 将pid出来的速度传到目标速度那里，进行速度限幅

 } 

void Chassis::adjust(){
    adjustPose();  // 得到x,y,z目标速度

    resolve();      // 由x,y,z的速度解算出底盘电机的pwm值
    
    for(int i = 0; i < 4; i++)
        motors[i].control();
}

void Chassis::setTargetVel(Vector3 vel){
    target_vel.x = constrain(vel.x, -max_vel_x, max_vel_x);
    target_vel.y = constrain(vel.y, -max_vel_y, max_vel_y);
    target_vel.z = constrain(vel.z, -max_vel_z, max_vel_z);
}

void Chassis::resolve(){
    
	float lf, rf, rb, lb;
    lf =  target_vel.x - target_vel.y - target_vel.z;
    rf =  target_vel.x + target_vel.y + target_vel.z;
    rb =  target_vel.x - target_vel.y + target_vel.z;
    lb =  target_vel.x + target_vel.y - target_vel.z;
	
	motors[LF].setTarget(lf);
	motors[RF].setTarget(rf);
	motors[RB].setTarget(rb);
	motors[LB].setTarget(lb);
}

void Chassis::testMotorForward(){
	for(int i = 0; i < 4; i++)
        motors[i].setPWM(5000);}

void Chassis::testMotorBackward(){
	for(int i = 0; i < 4; i++)
        motors[i].setPWM(-5000);	
}

void Chassis::testPinYi(int i){
    motors[LB].setPWM(i*4300);
    motors[LF].setPWM(i*(-4000));
    
    motors[RF].setPWM(i*4000);
    motors[RB].setPWM(i*(-4300));
	
    
}
void Chassis::testMotorMax(){
	for(int i = 0; i < 4; i++)
        motors[i].setPWM(9500);	
}

void Chassis::setMotorZero(){
	for(int i = 0; i < 4; i++)
        motors[i].setPWM(0);		
}

void Chassis::setMotorPwm(int i){
	for(int i = 0; i < 4; i++)
        motors[i].setPWM(i);		
}
